/**
   Copyright 2008 Kirill Ignatiev

    This file is part of kerr-wavefronts.

    kerr-wavefronts is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    kerr-wavefronts is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with kerr-wavefronts.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <cstdio>
#include <map>
#include <list>
#include <vector>
#include <GL/gl.h>
#include <gsl/gsl_math.h>
#include <cmath>
#include <cassert>
#include "settings.h"
#include "front.h"
#define loop(i,a) for (int i = 0; i < (signed int)(a); ++i)

#define Debug false
#define Flush do { fflush(stdout); fflush(stderr); } while(0)

typedef geodesic G;
typedef list<geodesic*> LG;
typedef list<double> LD;

#define foreachG(i,L) for (LG::iterator i = L.begin(); i != L.end(); ++i)
#define foreachGconst(i,L) for (LG::const_iterator i = L.begin(); i != L.end(); ++i)

wavefront::wavefront(ic_param* p1, const front_history& d)
  : param_gen(p1), data(d) {
  static const int default_num = 80;
  for (int i = 0; i < default_num; ++i) {
    double p = (double)i/(default_num-1);
    param.push_back(p);
    geodesics.push_back(param_gen->geod(p));
  }
  colourmap[0] = 1; colourmap[1] = 0.5; colourmap[2] = 1;
}

double angle(double n1x, double n1y, double n2x, double n2y) {
  double d1 = n1x*n1x + n1y*n1y;
  double d2 = n2x*n2x + n2y*n2y;
  double a = acos((n1x*n2x + n1y*n2y) / sqrt(d1) / sqrt(d2));
  assert(a>=0);
  return a;
}
double angle(point_t x0, point_t x1, point_t x2) {
  return angle(x1.x-x0.x, x1.y-x0.y, x2.x-x0.x, x2.y-x0.y);
}
double length(double x, double y) {
  x = fabs(x); y = fabs(y);
  double m = max(x,y);
  double m1 = min(x,y)/m;
  return m * sqrt(1 + m1*m1);
}
double dist2(double x1, double y1, double x2, double y2) {
  double d1 = fabs(x1-x2), d2 = fabs(y1-y2);
  double m1 = max(d1,d2);
  double m2 = min(d1,d2)/m1;
  return m1 * sqrt(1 + m2*m2);
}
double dist2(point_t p1, point_t p2) {
  return dist2(p1.x, p1.y, p2.x, p2.y);
}
double dist2(G* p1, G* p2) {
  return dist2(p1->x(), p1->y(), p2->x(), p2->y());
}
double distcheck(point_t p2, point_t p1, point_t p0) {
  double d1 = dist2(p1, p2);
  double d0 = dist2(p0, p1);
  return dist2(p0.x-p1.x, p0.y-p1.y,
               (p1.x-p2.x)/d1*d0, (p1.y-p2.y)/d1*d0);
}
double distcheck(G* g2, G* g1, G* g0) {
  double d1 = dist2(g1, g2);
  double d0 = dist2(g0, g1);
  double x0 = g0->x(), x1 = g1->x(), x2 = g2->x();
  double y0 = g0->y(), y1 = g1->y(), y2 = g2->y();
  return dist2(x0-x1, y0-y1, (x1-x2)/d1*d0, (y1-y2)/d1*d0);
}
double angle(G* g2, G* g1, G* g0) {
  double x0 = g0->x(), x1 = g1->x(), x2 = g2->x();
  double y0 = g0->y(), y1 = g1->y(), y2 = g2->y();
  return angle(x1-x2, y1-y2, x0-x2, y0-y2);
}
double curvature(G* g2, G* g1, G* g0) {
  double x0 = g0->x(), x1 = g1->x(), x2 = g2->x();
  double y0 = g0->y(), y1 = g1->y(), y2 = g2->y();
  double s01 = dist2(x0,y0, x1,y1);
  double s12 = dist2(x1,y1, x2,y2);
  double dx = ((x2-x1)/s12 - (x1-x0)/s01)/s01;
  double dy = ((y2-y1)/s12 - (y1-y0)/s01)/s01;
  return length(dx,dy);
}

void wavefront::evolve(double tmax) {
  int hist_steps =
    max(0, min((int)((tmax - data.current_time())/data.delay), data.N-1));
  for (int i = hist_steps; i >= 0; --i)
    evolve_once(tmax - i * data.delay);
}

void wavefront::evolve_once(double tmax) {
  if (tmax < data.current_time()) return;
  foreachG(i,geodesics) (*i)->evolve(tmax);

  static const double epstheta = 0.15708;
  static const double epsdist = 1.0; //0.312;

  // If there are too many geodesics, it is likely that
  //  we have run out of double precision
  if (geodesics.size() <= 4) return;
  static bool too_many_geods = false;
  if (geodesics.size() > 2000) {
    if (!too_many_geods)
      fprintf(stderr, "There are too many geodesics (%d).\n", geodesics.size());
    too_many_geods = true;
    data.update(tmax, geodesics, param);
    return;
  }

  LG newgeods;
  LD newpar;
  
  int original_size = geodesics.size();
  int count_removed = 0, count_added = 0;
  // I will now add new geodesics and prune old unnecessary ones
  
  // Iteration on consecutive triples of geodesics
  // g2 -- g1 -- g0
  // I will determine whether to keep all three, or add two more
  //  between g2&g1 and g1&g0
  G* g2 = *geodesics.begin(); geodesics.pop_front();
  G* g1 = *geodesics.begin(); geodesics.pop_front();
  newgeods.push_back(g2); newgeods.push_back(g1);
  double p2 = *param.begin(); param.pop_front();
  double p1 = *param.begin(); param.pop_front();
  newpar.push_back(p2); newpar.push_back(p1);

  int ignoreDarkPoints =
    Gets("correct-brightness",0) && !Gets("draw-wavefront-points",0);

  int i = 2;
  while (!geodesics.empty()) {
    G* g0 = *geodesics.begin(); geodesics.pop_front();
    double p0 = *param.begin(); param.pop_front();
    // Some heuristics to help decide which geodesics to keep/insert
    bool allow_p = fabs(p0-p2) < 0.1; // initial geods are on a line sometimes
    // kappa is the first curvature of the parabola through g2,g1,g0
    double kappa = curvature(g2,g1,g0);
    // dist is the distance between g1 and the line segment [g2,g0]
    double dist = distcheck(g2,g1,g0);
    
    // The calculations with theta/alpha give NaNs when the program runs
    //  out of precision
//     double theta = 2 * asin(sqrt(kappa*dist)/2);
//     double alpha = atan2(sin(theta), cos(theta)-1);

    // If the parabola [g2,g1,g0] looks smooth enough to be drawn as a line
    //  [g2,g0], then I may remove g1.
    // In practice, this is unstable, geodesics added/deleted all the time.
    // Bug: find a way to stabilise deletion of geodesics
    if (ignoreDarkPoints && p0-p2 < 1e-3/Wavefront_intensity) {
      // remove geodesic g1;
      newgeods.pop_back(); newgeods.push_back(g0);
      newpar.pop_back(); newpar.push_back(p0);
      g1 = g0; p1 = p0;
      count_removed++;
      continue;
    } else if (dist > epsdist/Wavefront_precision && g0->not_on_horizon()) {
      // If the parabola [g2,g1,g0] cannot be approximated well by two
      //  line segments, then bisect the segments [g2,g1], [g1,g0].
      double p21 = (p2+p1)/2; G* g21 = param_gen->geod(p21);
      double p10 = (p1+p0)/2; G* g10 = param_gen->geod(p10);
      g21->evolve(tmax); g10->evolve(tmax);
      newgeods.pop_back(); newpar.pop_back();
      newgeods.push_back(g21); newgeods.push_back(g1); newgeods.push_back(g10); newgeods.push_back(g0);
      newpar.push_back(p21); newpar.push_back(p1); newpar.push_back(p10); newpar.push_back(p0);
      g2 = g10; p2 = p10;
      g1 = g0; p1 = p0;
      count_added++;
      continue;
    }

    // just add the geodesic
    newgeods.push_back(g0); newpar.push_back(p0);
    g2 = g1; p2 = p1;
    g1 = g0; p1 = p0;
  }

  if (Debug && (count_removed || count_added)) {
    printf("There are now %d = %d + %d - %d geodesics.\n", newgeods.size(),
           original_size, count_added, count_removed); Flush;
  }
  geodesics = newgeods; param = newpar;
  // The values in param should be ascendingly sorted
  if (Debug) for (LD::iterator i = param.begin(); i != param.end(); ++i) {
    LD::iterator j(i); ++j; if (j == param.end()) break;
    assert(*j >= *i);
  }

  data.update(tmax, geodesics, param);
}

void wavefront::render() {
  static const bool allow_points = false;
  static const bool allow_lines = true;
  
  // Set up colours and other
  int history_size = data.data.size();
  if (history_size == 0) return;
  double current_time = data.data.begin()->first;
  typedef front_history::const_iterator T;
  front_history::const_iterator last = data.data.end(); --last;

  double last_time = last->first;
#define Intensity_exp(t) ((history_size>1) ? (exp(-4.6 * (current_time - (t)) / (current_time - last_time))) : 1)
#define Intensity_log(t) ((history_size>1) ? ((t-last_time)/(current_time-last_time)) : 1)
  int False_intensity = Gets("false-intensity", 0);
  //if(False_intensity) printf("False intensity\n"); else printf("True intensity\n");
#define Intensity(t) (False_intensity ? Intensity_log(t) : Intensity_exp(t))

  int tensePower = Gets("correct-brightness");

  // Draw the black hole, and the origin of wavefronts
  { glPointSize(3.);
    glLineWidth(2.);
    glEnable(GL_POINT_SMOOTH);
    glBegin(GL_POINTS);
    glColor4d(1,1,1,0.6);
    glVertex2d(0,0);
    glEnd();
    glBegin(GL_LINE_STRIP);
    foreachG(i,geodesics) glVertex2d((*i)->start_x, (*i)->start_y);
    glEnd();
    glBegin(GL_POINTS);
    foreachG(i,geodesics) glVertex2d((*i)->start_x, (*i)->start_y);
    glEnd(); }

  glLineWidth(1.);
  if (Gets("draw-wavefront-points", 0)) {
    glPointSize(1.);
    glBegin(GL_POINTS);
    for (T x = data.data.begin(); x != data.data.end(); ++x) {
      const vector<double>& points = *((*x).second);
      colourmap[3] = Intensity(x->first);
      glColor4dv(colourmap);
      for (int i = 0; i < points.size()/3; ++i)
        glVertex3d(points[3*i], points[3*i+1], 0);
    }
    glEnd();
  }
  if (Gets("draw-wavefront-lines", 1)) {
    for(T x = data.data.end(); x != data.data.begin();) {
      --x;
      const vector<double>& points = *(x->second);
      glBegin(GL_LINE_STRIP);
      int N = points.size()/3;
      if (N <= 1) continue;
      for (int i = 0; i < N; ++i) {
        colourmap[3] = Intensity(x->first);
        if (tensePower) {
          colourmap[3] *=
            min(1.0, Wavefront_intensity *
                (points[3*min(i+1,N-1)+2] - points[3*max(i-1,0)+2]));
        }
        glColor4dv(colourmap);
        glVertex3d(points[3*i], points[3*i+1], 0);
      }
      glEnd();
    }
  }
  if (Gets("draw-history", 1)) {
    front_history::const_iterator x = data.data.begin();
    front_history::const_iterator y = data.data.begin(); ++y;
    for (; y != data.data.end(); ++x, ++y) {
      const vector<double>& Px = *(x->second);
      const vector<double>& Py = *(y->second);
      int Nx = Px.size()/3, Ny = Py.size()/3;
      if (Nx + Ny <= 2) continue;
#define A(a,b) (a[3*(b)+2])
#define VertexX(i) do {                      \
        colourmap[3] = Intensity(x->first);                   \
        if (tensePower) colourmap[3] *=\
                          min(1.0, Wavefront_intensity *                    \
                              (Px[3*min(i+1,Nx-1)+2] - Px[3*max(i-1,0)+2])); \
        glColor4dv(colourmap);\
        glVertex3d(Px[3*i], Px[3*i+1], 0);      \
    } while(0)
#define VertexY(i) do {\
        colourmap[3] = Intensity(y->first);\
        if (tensePower)                    \
          colourmap[3] *=                                       \
            min(1.0, Wavefront_intensity *                                  \
                (Py[3*min(i+1,Ny-1)+2] - Py[3*max(i-1,0)+2]));         \
        glColor4dv(colourmap);\
        glVertex3d(Py[3*i], Py[3*i+1], 0);\
      } while(0)

      glBegin(GL_TRIANGLES);
      int Vi = 0, Vj = 0;
      int i=0, j=0;
      while (i < Nx-1 && j < Ny-1) {
        double nTx = A(Px,i+1), nTy = A(Py,j+1);
        VertexY(Vj);
        VertexX(Vi);
        if (nTx < nTy) {
          i++; Vi++; VertexX(i);
        } else {
          j++; Vj++; VertexY(j);
        }
      }
      while (++i < Nx) {
        VertexY(Vj);
        VertexX(Vi);
        VertexX(i);
        Vi++;
      }
      while (++j < Ny) {
        VertexY(Vj); VertexX(Vi); VertexY(j);
        Vj++;
      }
      glEnd();

    }
//     for(T x = data.data.end(); x != data.data.begin();) {
//       --x;
//       const vector<double>& points = *(x->second);
//       glBegin(GL_LINE_STRIP);
//       int N = points.size()/3;
//       if (N <= 1) continue;
//       for (int i = 0; i < N; ++i) {
//         colourmap[3] = Intensity(x->first);
//         if (tensePower) {
//           colourmap[3] *=
//             min(1.0, Wavefront_intensity *
//                 (points[3*min(i+1,N-1)+2] - points[3*max(i-1,0)+2]));
//         }
//         glColor4dv(colourmap);
//         glVertex3d(points[3*i], points[3*i+1], 0);
//       }
//       glEnd();
//     }
  }
}

// History

void front_history::update(double time, const list<geodesic*>& geods,
                           const list<double>& params) {
  vector<double>* d;
  int ngeods = geods.size();
  if (size == 0) {
    d = new vector<double>(ndim*ngeods);
    data.push_front(pair<double, vector<double>* >(time, d));
    ++size;
  } else if (prev_time + delay > time) {
    data.begin()->first = time;
    d = data.begin()->second;
  } else {
    prev_time += delay;
    if (size == N) {
      history_rep::iterator i = data.end(); --i;
      d = i->second;
      data.pop_back();
      data.push_front(pair<double, vector<double>* >(time, d));
    } else {
      ++size;
      d = new vector<double>(ndim*ngeods);
      data.push_front(pair<double, vector<double>* >(time, d));
    }
  }
  if (d->size() != ndim*ngeods)
    d->resize(ndim*ngeods);
  int i = 0;
  LG::const_iterator g = geods.begin();
  LD::const_iterator p = params.begin();
  while (g != geods.end()) {
    (*d)[i++] = (*g)->x();
    (*d)[i++] = (*g)->y();
    (*d)[i++] = *p;
    ++p;
    ++g;
  }
}

// Initial conditions

geodesic* line_param::geod(double j) {
  double h = length *(2*j-1);
  double r = sqrt(h*h+r0*r0);
  double alpha = atan2(h, r0);
  double rdot0 = -cos(alpha);
  double phidot0 = sin(alpha)/r;
  geodesic* g = new schw_geodesic(r, alpha, rdot0, phidot0);
  g->t = this->t0;
  return g;
}

geodesic* circle_param::geod(double j) {
  double alpha = M_PI + angle*(j-0.5);
  double rdot0 = cos(alpha);
  double phidot0 = sin(alpha)/r0;
  geodesic* g = new schw_geodesic(r0, phi0, rdot0, phidot0);
  g->t = this->t0;
  return g;
}

geodesic* kerr_line_param::geod(double j) {
  double a = p.a, Q = p.Q;
  double h = length * (2*j-1);
  double r = sqrt(h*h+r0*r0);
  double phi0 = atan2(h,r0);

  double Delta = r*r-r+Q*Q+a*a;
  double pr0 = -cos(phi0)*r*r/Delta;

  //p.l = (a - a/Delta*(r*r+a*a) + r*sin(phi0)) / (1-a*a/Delta);
  double l = (Delta*r*sin(phi0) - a*(r-Q*Q))/(Delta-a*a);
  p.l = l;
  p.rminus = 0.5 - sqrt(0.25-(Q*Q+a*a));
  p.rplus = 0.5 + sqrt(0.25-(Q*Q+a*a));

  double rdot0 = -cos(phi0);
  double H = -(r*r+a*a-a*l)/Delta/(r*r) + (l-a)*(l-a)/(r*r) + r*r/Delta*rdot0*rdot0;
  //printf("Initial lagrangian is %f.\n", H);
  geodesic* g =  new kerr_geodesic(p, r, phi0, pr0);
  g->t = this->t0;
  return g;
}


geodesic* kerr_circle_param::geod(double j) {
  double a = p.a, Q = p.Q, r=r0;
  double Delta = r*r-r+Q*Q+a*a;
  double pr0 = -cos(phi0)*r*r/Delta;
  double alpha = M_PI + angle*(j-0.5);
  double rdot0 = cos(alpha);
  double phidot0 = sin(alpha)/r0;
  double l = (Delta*r*r*phidot0 - a*(r-Q*Q))/(Delta-a*a);
  p.l = l;
  p.rminus = 0.5 - sqrt(0.25-(Q*Q+a*a));
  p.rplus = 0.5 + sqrt(0.25-(Q*Q+a*a));
  geodesic* g = new kerr_geodesic(p, r0, phi0, rdot0);
  g->t = this->t0;
  return g;
}
